from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare


# "/ugv/actuators/chassis/out/motor",
def generate_launch_description():
    return LaunchDescription(
        [
            # 接收 rtk
            Node(
                package="rtk",
                executable="read_rtk",
                name="read_rtk",
                output="screen",
                parameters=[
                    {
                        "rtk_port": "/dev/ttyUSB0",
                        "rtk_baudrate": 115200,
                        "rtk_timeout": 0.02,
                    }
                ],
                remappings=[
                    ("/rtk_lla_info", "/ugv/sensors/rtk/out/lla"),
                ],
            ),
            # 发送 can
            IncludeLaunchDescription(
                PythonLaunchDescriptionSource(
                    PathJoinSubstitution(
                        [
                            FindPackageShare("ros2_socketcan"),
                            "launch",
                            "socket_can_sender.launch.py",
                        ]
                    )
                ),
                launch_arguments=[
                    ("interface", "can0"),
                    ("timeout_sec", "0.05"),
                    ("to_can_bus_topic", "/ugv/actuators/chassis/in/to_can_bus"),
                ],
            ),
            # 接收 can
            IncludeLaunchDescription(
                PythonLaunchDescriptionSource(
                    PathJoinSubstitution(
                        [
                            FindPackageShare("ros2_socketcan"),
                            "launch",
                            "socket_can_receiver.launch.py",
                        ]
                    )
                ),
                launch_arguments=[
                    ("interface", "can0"),
                    ("interval_sec", "0.05"),
                    ("filters", "178:FFF,188:FFF,051:FFF,701:FFF,120:FFF"),
                    ("from_can_bus_topic", "/ugv/actuators/chassis/out/from_can_bus"),
                ],
            ),
            # 解析can
            Node(
                package="chassis",
                executable="chassis",
                name="chassis",
                output="screen",
                remappings=[
                    # from control node
                    ("/to_can_bus_cmd", "/ugv/actuators/chassis/in/to_can_bus_cmd"),
                    # from chassis node
                    (
                        "/from_can_bus_cmd",
                        "/ugv/actuators/chassis/out/from_can_bus_cmd",
                    ),
                    ("/motor_data", "/ugv/actuators/chassis/out/motor_data"),
                    ("/target_vel", "/ugv/actuators/chassis/out/target_vel"),
                    ("/steer_data", "/ugv/actuators/chassis/out/steer_data"),
                    # socketcan node
                    ("/from_can_bus", "/ugv/actuators/chassis/out/from_can_bus"),
                    ("/to_can_bus", "/ugv/actuators/chassis/in/to_can_bus"),
                ],
            ),
            Node(
                package="sls_controller",
                executable="sls_mainController",
                name="sls_controller",
                output="screen",
                parameters=[{"vehicle_class": "UGV", "vehicle_id": "ugv1"}],
            ),
        ]
    )
